using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Services.SubscriptionManager;
using W3C.Soap;

using System;
using System.Collections.Generic;
using System.ComponentModel;
using dssphttp = Microsoft.Dss.Core.DsspHttp;
using pxdrive = Microsoft.Robotics.Services.Drive.Proxy;
using xml = System.Xml;
using xna = Microsoft.Xna.Framework;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using simtypes = Microsoft.Robotics.Simulation;
using simengine = Microsoft.Robotics.Simulation.Engine;
using physics = Microsoft.Robotics.Simulation.Physics;
using jaguar = SimJaguar.Jaguar;
using Microsoft.Robotics.PhysicalModel;


namespace SimJaguar.QuadDifferentialDrive
{

    /// <summary>
    /// Simulated Quad Differential Drive Service
    /// </summary>
    [DisplayName("Simulated QuadDifferentialDrive")]
    [Description("A four-wheel drive version of the differential drive service.")]
    [Contract(Contract.Identifier)]
    [AlternateContract(pxdrive.Contract.Identifier)]
    public class SimulatedQuadDifferentialDriveService : DsspServiceBase
    {
        #region Simulation Variables
        jaguar.JaguarEntity _entity;
        simengine.SimulationEnginePort _notificationTarget;
        #endregion

        /// <summary>
        /// Microsoft.Robotics.Services.Drive.SimulatedQuadDifferentialDrive Partner
        /// </summary>
        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways, Optional = false)]
        private SubscriptionManagerPort _subMgrPort = new Microsoft.Dss.Services.SubscriptionManager.SubscriptionManagerPort();

        // Port for receiving generic differential drive commands
        [AlternateServicePort(
            AllowMultipleInstances = true,
            AlternateContract = pxdrive.Contract.Identifier)]
        private pxdrive.DriveOperations _diffDrivePort = new Microsoft.Robotics.Services.Drive.Proxy.DriveOperations();

        /// <summary>
        /// Main service port for quad drive commands
        /// </summary>
        [ServicePort("/simulatedquaddifferentialdrive", AllowMultipleInstances = true)]
        private QuadDriveOperations _mainPort = new QuadDriveOperations();

        /// <summary>
        /// SimJaguar.QuadDifferentialDrive.SimulatedQuadDifferentialDriveService State
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = "SimulatedQuadDifferentialDriveService.Config.xml")]
        private DriveDifferentialFourWheelState _state = new DriveDifferentialFourWheelState();

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public SimulatedQuadDifferentialDriveService(DsspServiceCreationPort creationPort)
            :
                base(creationPort)
        {
        }

        #region Initialization and State
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            if (_state == null)
                CreateDefaultState();

            // Find our simulation entity that represents the "hardware" or real-world service.
            // To hook up with simulation entities we do the following steps
            // 1) have a manifest or some other service create us, specifying a partner named SimulationEntity
            // 2) in the simulation service (us) issue a subscribe to the simulation engine looking for
            //    an instance of that simulation entity. We use the Entity.State.Name for the match so it must be
            //    exactly the same. See SimulationTutorial2 for the creation process
            // 3) Listen for a notification telling us the entity is available
            // 4) cache reference to entity and communicate with it issuing low level commands.

            _notificationTarget = new simengine.SimulationEnginePort();

            // PartnerType.Service is the entity instance name. 
            simengine.SimulationEngine.GlobalInstancePort.Subscribe(ServiceInfo.PartnerList, _notificationTarget);

            // don't start listening to DSSP operations, other than drop, until notification of entity
            Activate(new Interleave(
                new TeardownReceiverGroup
                (
                    Arbiter.Receive<simengine.InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
                    Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DefaultDropHandler),
                    Arbiter.Receive<DsspDefaultDrop>(false, _diffDrivePort, DefaultDropHandler)
                ),
                new ExclusiveReceiverGroup(),
                new ConcurrentReceiverGroup()
            ));
        }

        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> SubscribeHandler(Subscribe subscribe)
        {
            SubscribeRequestType request = subscribe.Body;
            LogInfo("Subscribe request from: " + request.Subscriber);

            // Use the Subscription Manager to handle the subscribers
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, request, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    // Send a notification on successful subscription so that the
                    // subscriber can initialize its own state
                    base.SendNotificationToTarget<Replace>(request.Subscriber, _subMgrPort, _state);
                },
                delegate(Exception e)
                {
                    LogError(null, "Subscribe failed", e);
                }
            );

            yield break;
        }

        void CreateDefaultState()
        {
            _state = new DriveDifferentialFourWheelState();
            _state.LeftWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
            _state.RightWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
            _state.LeftWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
            _state.RightWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
            _state.LeftWheel.EncoderState = new Microsoft.Robotics.Services.Encoder.Proxy.EncoderState();
            _state.RightWheel.EncoderState = new Microsoft.Robotics.Services.Encoder.Proxy.EncoderState();
            _state.RearLeftWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
            _state.RearRightWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
            _state.RearLeftWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
            _state.RearRightWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
        }

        void UpdateStateFromSimulation()
        {
            if (_entity != null)
            {
                _state.TimeStamp = DateTime.Now;
                _state.LeftWheel.MotorState.CurrentPower = _entity.FrontLeftWheel.Wheel.MotorTorque;
                _state.RightWheel.MotorState.CurrentPower = _entity.FrontRightWheel.Wheel.MotorTorque;
                _state.RearLeftWheel.MotorState.CurrentPower = _entity.RearLeftWheel.Wheel.MotorTorque;
                _state.RearRightWheel.MotorState.CurrentPower = _entity.RearRightWheel.Wheel.MotorTorque;
                _state.LeftWheel.EncoderState.CurrentReading = _entity.FrontLeftEncoder;
                _state.RightWheel.EncoderState.CurrentReading = _entity.FrontRightEncoder;
                _state.Position = _entity.State.Pose.Position;
            }
        }
        #endregion

        #region Entity Handlers
        void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
        {
            // Publish the service to the local Node Directory
            DirectoryInsert();

            InsertEntityNotificationHandler(ins);

            //SpawnIterator(TestDriveDistanceAndRotateDegrees);
        }

        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            _entity = (jaguar.JaguarEntity)ins.Body;
            _entity.ServiceContract = Contract.Identifier;

            // create default state based on the physics entity
            xna.Vector3 separation = _entity.FrontLeftWheel.Position - _entity.FrontRightWheel.Position;
            _state.DistanceBetweenWheels = separation.Length();

            _state.LeftWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
            _state.RightWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;

            // enable other handlers now that we are connected
            Activate(new Interleave(
                new TeardownReceiverGroup
                (
                    Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DefaultDropHandler),
                    Arbiter.Receive<DsspDefaultDrop>(false, _diffDrivePort, DefaultDropHandler)
                ),
                new ExclusiveReceiverGroup
                (
                    Arbiter.Receive<SetPose>(true, _mainPort, SetPoseHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.DriveDistance>(true, _diffDrivePort, DriveDistanceHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.RotateDegrees>(true, _diffDrivePort, RotateHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.SetDrivePower>(true, _diffDrivePort, SetPowerHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.SetDriveSpeed>(true, _diffDrivePort, SetSpeedHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.AllStop>(true, _diffDrivePort, AllStopHandler),
                    Arbiter.Receive<simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
                    Arbiter.Receive<simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler)
                ),
                new ConcurrentReceiverGroup
                (
                    Arbiter.ReceiveWithIterator<dssphttp.HttpGet>(true, _mainPort, MainPortHttpGetHandler),
                    Arbiter.ReceiveWithIterator<Get>(true, _mainPort, MainPortGetHandler),
                    Arbiter.ReceiveWithIterator<dssphttp.HttpGet>(true, _diffDrivePort, HttpGetHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.Get>(true, _diffDrivePort, GetHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.Subscribe>(true, _diffDrivePort, SubscribeHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.ReliableSubscribe>(true, _diffDrivePort, ReliableSubscribeHandler),
                    Arbiter.ReceiveWithIterator<pxdrive.EnableDrive>(true, _diffDrivePort, EnableHandler)
                )
            ));
        }

        void DeleteEntityNotificationHandler(simengine.DeleteSimulationEntity del)
        {
            _entity = null;

            // disable other handlers now that we are no longer connected to the entity
            Activate(new Interleave(
                new TeardownReceiverGroup
                (
                    Arbiter.Receive<simengine.InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
                    Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DefaultDropHandler),
                    Arbiter.Receive<DsspDefaultDrop>(false, _diffDrivePort, DefaultDropHandler)
                ),
                new ExclusiveReceiverGroup(),
                new ConcurrentReceiverGroup()
            ));
        }
        #endregion

        #region Subscribe Handling
        public IEnumerator<ITask> SubscribeHandler(pxdrive.Subscribe subscribe)
        {
            Activate(Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    _subMgrPort.Post(new submgr.Submit(
                        subscribe.Body.Subscriber, DsspActions.UpdateRequest, _state, null));
                },
                delegate(Exception ex) { LogError(ex); }
            ));

            yield break;
        }

        public IEnumerator<ITask> ReliableSubscribeHandler(pxdrive.ReliableSubscribe subscribe)
        {
            Activate(Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    _subMgrPort.Post(new submgr.Submit(
                        subscribe.Body.Subscriber, DsspActions.UpdateRequest, _state, null));
                },
                delegate(Exception ex) { LogError(ex); }
            ));
            yield break;
        }
        #endregion

        #region _mainPort handlers
        public IEnumerator<ITask> MainPortHttpGetHandler(dssphttp.HttpGet get)
        {
            UpdateStateFromSimulation();
            get.ResponsePort.Post(new dssphttp.HttpResponseType(_state));
            yield break;
        }

        public IEnumerator<ITask> MainPortGetHandler(Get get)
        {
            UpdateStateFromSimulation();
            get.ResponsePort.Post(_state);
            yield break;
        }

        public void SetPoseHandler(SetPose setPose)
        {
            if (_entity == null)
                throw new InvalidOperationException("Simulation entity not registered with service");

            Task<jaguar.JaguarEntity, Pose> task = new Task<jaguar.JaguarEntity, Pose>(_entity, setPose.Body.EntityPose, SetPoseDeferred);
            _entity.DeferredTaskQueue.Post(task);
        }

        void SetPoseDeferred(jaguar.JaguarEntity entity, Pose pose)
        {
            entity.PhysicsEntity.SetPose(pose);
        }

        #endregion

        #region _diffDrivePort handlers
        public IEnumerator<ITask> HttpGetHandler(dssphttp.HttpGet get)
        {
            UpdateStateFromSimulation();
            pxdrive.DriveDifferentialTwoWheelState _twoWheelState =
                (pxdrive.DriveDifferentialTwoWheelState)((pxdrive.DriveDifferentialTwoWheelState)_state).Clone();
            get.ResponsePort.Post(new dssphttp.HttpResponseType(_twoWheelState));
            yield break;
        }

        public IEnumerator<ITask> GetHandler(pxdrive.Get get)
        {
            UpdateStateFromSimulation();
            pxdrive.DriveDifferentialTwoWheelState _twoWheelState =
                (pxdrive.DriveDifferentialTwoWheelState)((pxdrive.DriveDifferentialTwoWheelState)_state).Clone();
            get.ResponsePort.Post(_twoWheelState);
            yield break;
        }

        public IEnumerator<ITask> DriveDistanceHandler(pxdrive.DriveDistance driveDistance)
        {
            SuccessFailurePort entityResponse = new SuccessFailurePort();
            _entity.DriveDistance(
                (float)driveDistance.Body.Distance,
                (float)driveDistance.Body.Power,
                entityResponse);

            yield return Arbiter.Choice(entityResponse,
                delegate(SuccessResult s) { driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance); },
                delegate(Exception e)
                {
                    driveDistance.ResponsePort.Post(new W3C.Soap.Fault());
                });

            yield break;
        }

        public IEnumerator<ITask> RotateHandler(pxdrive.RotateDegrees rotate)
        {
            SuccessFailurePort entityResponse = new SuccessFailurePort();
            _entity.RotateDegrees(
                (float)rotate.Body.Degrees,
                (float)rotate.Body.Power,
                entityResponse);

            yield return Arbiter.Choice(entityResponse,
                delegate(SuccessResult s) { rotate.ResponsePort.Post(DefaultUpdateResponseType.Instance); },
                delegate(Exception e)
                {
                    rotate.ResponsePort.Post(new W3C.Soap.Fault());
                });

            yield break;
        }

        public IEnumerator<ITask> SetPowerHandler(pxdrive.SetDrivePower setPower)
        {
            if (_entity == null)
                throw new InvalidOperationException("Simulation entity not registered with service");

            // Call simulation entity method for setting wheel torque
            _entity.SetMotorTorque(
                (float)(setPower.Body.LeftWheelPower),
                (float)(setPower.Body.RightWheelPower));

            UpdateStateFromSimulation();
            setPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            // send update notification for entire state
            _subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
            yield break;
        }

        public IEnumerator<ITask> SetSpeedHandler(pxdrive.SetDriveSpeed setSpeed)
        {
            if (_entity == null)
                throw new InvalidOperationException("Simulation entity not registered with service");

            _entity.SetVelocity(
                (float)setSpeed.Body.LeftWheelSpeed,
                (float)setSpeed.Body.RightWheelSpeed);

            UpdateStateFromSimulation();
            setSpeed.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            // send update notification for entire state
            _subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
            yield break;
        }

        public IEnumerator<ITask> EnableHandler(pxdrive.EnableDrive enable)
        {
            if (_entity == null)
                throw new InvalidOperationException("Simulation entity not registered with service");

            _state.IsEnabled = enable.Body.Enable;
            _entity.IsEnabled = _state.IsEnabled;

            UpdateStateFromSimulation();
            enable.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            // send update for entire state
            _subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
            yield break;
        }

        public IEnumerator<ITask> AllStopHandler(pxdrive.AllStop estop)
        {
            if (_entity == null)
                throw new InvalidOperationException("Simulation entity not registered with service");

            _entity.SetMotorTorque(0, 0);
            _entity.SetVelocity(0);

            UpdateStateFromSimulation();
            estop.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            // send update for entire state
            _subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
            yield break;
        }
        #endregion

        // Test the DriveDistance and RotateDegrees messages
        public IEnumerator<ITask> TestDriveDistanceAndRotateDegrees()
        {
            Random rnd = new Random();
            bool success = true;

            // drive in circles
            while (success)
            {
                double distance = rnd.NextDouble() * 1 + 0.5;
                double angle = rnd.NextDouble() * 90 - 45;

                // first leg
                yield return Arbiter.Choice(
                    _diffDrivePort.RotateDegrees(angle, 0.2),
                    delegate(DefaultUpdateResponseType response) { },
                    delegate(Fault f) { success = false; }
                );

                yield return Arbiter.Choice(
                    _diffDrivePort.DriveDistance(distance, 0.2),
                    delegate(DefaultUpdateResponseType response) { },
                    delegate(Fault f) { success = false; }
                );

                // return
                yield return Arbiter.Choice(
                    _diffDrivePort.RotateDegrees(180, 0.2),
                    delegate(DefaultUpdateResponseType response) { },
                    delegate(Fault f) { success = false; }
                );

                yield return Arbiter.Choice(
                    _diffDrivePort.DriveDistance(distance, 0.2),
                    delegate(DefaultUpdateResponseType response) { },
                    delegate(Fault f) { success = false; }
                );

                // reset position
                yield return Arbiter.Choice(
                    _diffDrivePort.RotateDegrees(180 - angle, 0.2),
                    delegate(DefaultUpdateResponseType response) { },
                    delegate(Fault f) { success = false; }
                );
            }
        }
    }
}



